# ==========================================================================#
#                          基站部署算法（转MATLAB）
# 功能：1.根据智能体当前位置计算出下一时刻位置
#      2.计算出基站位置
#      3.为智能体分配基站位置
#      4.展示智能体覆盖区域
# ==========================================================================#
import math

import numpy

import Detect_obs
import Obs_vfrict
import Coverage_value


# ============== 区域网格化 ===============#
Estart_x = 0;  Estart_y = 0;
Weight = 49.9;  High = 49.9;
dL = 0.1   # 设置网格大小
xL = numpy.linspace(0,Weight,round(Weight/dL)+1)  # 因为这里由于二进制可能不准确的问题会导致结果不为整数，所以加上round
yL = numpy.linspace(0,High,round(High/dL)+1)
XL = []
for i in range(len(xL)):
    XL.append(xL)
YL = numpy.array(XL).T
C = numpy.zeros((len(xL), len(yL)), dtype=numpy.int)  # 存储网格的高度值
C[0,:] = 100;   C[:,0] = 100;   # 墙壁赋覆盖值
C[-1,:] = 100;  C[:,-1] = 100;

# ============== 全局参数定义 ===============#
Rnum = 33
robot = []
t0 = 0.1               # 单位时间
id = 9                 # 基站信号半径
gd = 1                 # 非候选智能体对于基站的过渡距离
stable_iter = 20       # 前期稳定循环次数
flag = 2               # 为1表示选择好了候选智能体，为2表示布置好了全部的新基站
task_result = []       # 保存候选智能体和其目标坐标
BS = [[0, 0, 0]]       # 基站列表（下标 x坐标 y坐标）
BS_last = 1            # 之前部署的基站中的最后一个所在BS列表中的行号
BS_current = 1         # 最后一个基站的行号
BS_Dire = []           # 保存有指引方向基站信息
bots = []              # 保存候选智能体
Robots_XData = []      # 保存所有智能体的X坐标
Robots_YData = []      # 保存所有智能体的Y坐标

# =============================================== #
#          根据智能体当前位置计算下一时刻位置
# 参数：
#      1.智能体对象列表
#      2.基站列表
# =============================================== #
def solve_robots_next_position(robot, BS):
    # =========== 智能体运动 =========== #
    for i in range(0,len(robot)):
        # ------- 如果该智能体已经作为基站，则跳过 ------- #
        if i in BS[:,1]:
            continue
        dxv = 0
        dyv = 0
        # 1.邻居之间的排斥与吸引
        for j in range(0,Rnum+1):
            if i != j:
                # 智能体间的排斥与吸引
                robot[i].Ox = robot[j].x
                robot[i].Oy = robot[j].y
                robot[i].vo = robot[j].v
                robot[i].vo_k = robot[j].v_k
                if robot[i].r() <= robot[i].r0_frict:
                    dxv = dxv + robot[i].v_rep() * math.cos(robot[i].th())
                    dyv = dyv + robot[i].v_rep() * math.sin(robot[i].th())

        # 2.障碍物和墙壁的排斥
        (D, xm, ym, dm) = Detect_obs.detect_obs(robot[i].x, robot[i].r0_ob_rep, C, dL, 100)
        if xm != 9999:
            f1, f2 = Obs_vfrict.obs_vfrict(robot[i], xm, ym, dL, 2)
            dxv = dxv + f1
            dyv = dyv + f2

        # 3.智能体朝向基站候选点运动
        bots_flag = 0     # 候选智能体是否就位
        # 判断是否包含在候选智能体列表中
        if flag == 1 and numpy.argwhere(bots == i):
            # 3.1 设置智能体运动目标
            row_index = numpy.argwhere(numpy.array(task_result)[:,1] == i)
            robot[i].Ox = task_result[row_index[0][0], 2]
            robot[i].Oy = task_result[row_index[0][0], 3]
            robot[i].p_ob_prep = 0.2  # 减小障碍物（墙壁）的排斥力
            if robot[i].r() > 0.3:
                dxv = dxv - 1.7 * math.cos(robot[i].th())
                dxv = dyv - 1.7 * math.sin(robot[i].th())
            # 3.2 添加新基站
            bots_flag = 1
            task_result[row_index[0][0], 4] = 1
            # map(list, zip(*m)) 矩阵转置   axis=1插入列   axis=0插入行
            BS = numpy.insert(BS, len(BS), [i, robot[i].x, robot[i].y], axis=0)
            #### p1(i).MarkerFaceColor = 'r';  %变为红色
    ######### 设置基站覆盖值
            # h_g = rectangle('position', [robot(i).x - id, robot(i).y - id, id * 2, id * 2], 'curvature', [1, 1],
            #                 'edgecolor', 'y', 'Curvature', 1, 'facecolor', 'y'); % 画圆
            # uistack(h_g, 'bottom'); % 置于底层
            C = Coverage_value.coverage_value(round(robot[i].x/dL),round(robot[i].y/dL),round(id/dL),C,2,50,49)
            # 基站连线
            dis = math.sqrt((BS[-1][2]-numpy.array(BS)[1:BS_current+1,2])**2 + (BS[-1][3]-numpy.array(BS)[1:BS_current+1,3])**2)
            link_index = numpy.argwhere(numpy.array(dis) <= id + 2)  # 距离小于中继距离的基站下标
    ######### 连线
            # *****
            # 3.3 如果候选智能体都成为了基站，则将flag置2，进行下一轮基站候选点选择
            if sum(numpy.array(task_result)[:][4]) == numpy.array(bots).shape[0]:
                flag = 2
                if BS_last != BS_current:
                    BS_last = BS_current
                BS_last = BS_last + 1
                BS_current = BS.shape[0]  # BS_last与BS_current之间是上一轮增加的基站

            # 4.非候选智能体朝着基站指引方向运动
            if numpy.array(bots).shape[0] > 0 and i in numpy.array(bots):
                if numpy.array(BS_Dire).shape[0] > 0:
                    # 计算自己与所有基站的距离（实机表现为智能体实际能获取到的所有基站距离）
                    dis = numpy.square((robot[i].x-numpy.array(BS_Dire)[:,2])**2 + (robot[i].y-numpy.array(BS_Dire)[:,3])**2)
                    # 找出距离最小的
                    min_value = min(dis.tolist())
                    min_index = numpy.argwhere(dis == min_value)
                    # 与基站距离小于中间点距离
                    if min_value[0] < math.sqrt((robot[i].x-BS_Dire[min_index[0,0],4])**2 + (robot[i].x-BS_Dire[min_index[0,0],5])**2):
                        robot[i].v_k = BS_Dire(min_index[0,0],6)   # 更改运动方向
                        robot[i].v = robot[i].v + 0.5
                    else:
                        if abs(min_value[0]-id) <= 3:  # 非候选智能体活动范围（基站外圈）
                            robot[i].v = 0
                        else:
                            if min_value[0] < id:
                                robot[i].v_k = BS_Dire(min_index[0,0],6)   # 更改运动方向
                                robot[i].v = robot[i].v + 0.6
                            else:
                                robot[i].v_k = math.pi + BS_Dire(min_index[0,0],6)  # 更改运动方向
                                robot[i].v = robot[i].v + 0.6

            # 判断候选智能体是否就位，已经就位就无需再更新位置
            if bots_flag == 1:
                continue
            # 更新速度
            dxv = dxv + robot[i].v * math.cos(robot[i].v_k) + dxv
            dyv = dyv + robot[i].v * math.sin(robot[i].v_k) + dyv

            robot[i].v = math.sqrt(dxv**2 + dyv**2)

            # 速度控制
            if robot[i].v > 1.1:
                robot[i].v = 1.1

            # 新的速度方向
            robot[i].v_k = math.atan2(dyv, dxv)

            # 更新机器人位置
            Robots_XData.append(robot[i].x + (robot[i].v * math.cos(robot[i].v_k)) * t0)
            Robots_YData.append(robot[i].y + (robot[i].v * math.sin(robot[i].v_k)) * t0)
            robot[i].x = Robots_XData[-1]
            robot[i].y = Robots_YData[-1]
    ######### 更新机器人的实时位置，以便产生动画
    ######### 尾巴动画设置
            # *****

    # =========== 判断基站是否布置完成 =========== #
    od = numpy.argwhere(C == 0)
    if len(BS) == Rnum + 1 or len(od) == 0:
        return (1, robot)
        # =========== 2.1 选择新一轮基站候选点 =========== #
        



